
/*- Include lib interfaces: ANSI C, IUP and OpenGL ------*/
#include <stdio.h>
#include <stdlib.h>
#ifdef WIN32
#include <windows.h>    /* includes only in MSWindows not in UNIX */
#endif
#include <GL/glew.h>

#include <iup.h>        /* IUP functions*/
#include <iupgl.h>      /* IUP functions related to OpenGL (IupGLCanvasOpen,IupGLMakeCurrent and IupGLSwapBuffers) */

#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <cvctracker/cvc_tracker.h>
#include "sensor_view_render.h"
#include <iostream>

Renderer _render;

cv::SurfFeatureDetector detector( 600 );
cv::SurfDescriptorExtractor extractor;
cv::DescriptorMatcher* matcher  = new cv::BFMatcher( cv::NORM_L2, true );
CVCTracker cvct( &detector , &extractor , matcher );

double proj[16];
double mv[16];
double mvInv[16];
double cameraPos[3];
double cameraRot[9];

Ihandle *canvas;                    /* canvas handle */
Ihandle *msgbar;                    /* message bar  handle */
int width=640,height=480;           /* width and height of the canvas  */
cv::Mat _imageFile;

float _azimuth = 0;
float _pitch = 0;
float _roll = 0;

bool drawCube = false;

void getCameraPosition( double m[16], double mI[16], double trans[3], double rot[9] )
{
	cv::Mat modelview = ( cv::Mat_<double>( 4, 4 ) <<   m[0], m[1], m[2], m[3],   m[4], m[5], m[6], m[7],   m[8], m[9], m[10], m[11],   m[12], m[13], m[14], m[15] );
	cv::Mat modelviewinv = modelview.inv();

	memcpy( mI, modelviewinv.ptr<double>(), sizeof( double ) * 16 );

	trans[0] = modelviewinv.at<double>( 3, 0 );
	trans[1] = modelviewinv.at<double>( 3, 1 );
	trans[2] = modelviewinv.at<double>( 3, 2 );

	//rotacao da camera em coordenadas do mundo por linhas
	rot[0] = m[0]; rot[3] = m[4]; rot[6] = m[8];
	rot[1] = m[1]; rot[4] = m[5]; rot[7] = m[9];
	rot[2] = m[2]; rot[5] = m[6]; rot[8] = m[10];
}



/* function called when the canvas is exposed in the screen */
int repaint_cb( Ihandle* self ) 
{
	IupGLMakeCurrent( self );
	// the curve
	glClear( GL_DEPTH_BUFFER_BIT );
	glClear( GL_COLOR_BUFFER_BIT );
	glViewport( 0, 0, 640, 480 );


		float mvps[16];
		float mvp[16];
		cvct.setSensorValues( _azimuth, _pitch, _roll );
		cvct.getMVPSensor( mvps );
		
		_render.setMVP( mvps );
		//_render.setBackgroundImage( ( const char* )image.data, 640, 480 );
		_render.render(  );
		//getCameraPosition( mv, mvInv, cameraPos, cameraRot );
	
	IupGLSwapBuffers( self );
	return IUP_DEFAULT;
}



int repaintCanvasImageCallback( Ihandle* self )
{
	IupGLMakeCurrent( self );
	glClear( GL_DEPTH_BUFFER_BIT );
	glViewport( 0, 0, 640, 480 );
	return IUP_DEFAULT;
}



/* function called in the event of changes in the width or in the height of the canvas */
int resize_cb( Ihandle *self, int new_width, int new_height )
{
	IupGLMakeCurrent( self );

	static bool firstTime = true;
	if (firstTime)
	{
		_render.init();
		firstTime = false;
	}
	glShadeModel( GL_SMOOTH );                          // Enable Smooth Shading
	glClearColor( 0.0f, 0.0f, 0.0f, 0.5f );				// Black Background
	glEnable( GL_DEPTH_TEST );							// Enables Depth Testing
	glDepthFunc( GL_LEQUAL );						    // The Type Of Depth Testing To Do
	glEnable( GL_COLOR_MATERIAL );
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
	cvct.getGLProjectionMatrix( proj );
	//_render.setProj( proj );
	return IUP_DEFAULT; /* return to the IUP main loop */
}



int idle_cb( Ihandle* self )
{
	

	return IUP_DEFAULT;
}







int chessboardCallback(void)
{
	cvct.useNaturalMarker( false );
	cvct.useTracking( true );
	return IUP_DEFAULT;
}



int naturalMarkerCallback(void)
{
	cvct.useNaturalMarker( true );
	cvct.useTracking( true );
	return IUP_DEFAULT;
}



int restartCallback(void)
{
	 cvct.restartKLT();
     return IUP_DEFAULT;
}



int detectionCallback(void)
{
	cvct.useTracking( false );
	return IUP_DEFAULT;
}



int exit_cb(void)
{
     return IUP_CLOSE;
}



Ihandle* InitToolbar(void)
{
	Ihandle* toolbar;

	/* Create four buttons */
	Ihandle* openFile = IupButton("openFile", "openFileAction");
	Ihandle* chessboard = IupButton("chessboard", "chessboardAction");
	Ihandle* naturalMarker = IupButton("Natural Marker", "naturalMarkerAction");
	Ihandle* restart = IupButton("Restart", "restartAction");
	//Ihandle* changeColor = IupButton("Change Color", "changeColorAction");
	Ihandle* detection = IupButton("Detection", "detectionAction");

	/* Associate tip's (text that appear when the mouse is over) */
	IupSetAttribute(openFile,"TIP","Open Image");
	IupSetAttribute(chessboard,"TIP","Chessboard");
	IupSetAttribute(naturalMarker,"TIP","Natural Marker");
	IupSetAttribute(restart,"TIP","Restart");
	//IupSetAttribute(changeColor,"TIP","Change Color");
	IupSetAttribute(detection,"TIP","Detection");

	/* Associate function callbacks to the button actions */
	//IupSetFunction("openFileAction", (Icallback)openFileCallback);
	IupSetFunction("chessboardAction", (Icallback)chessboardCallback);
	IupSetFunction("naturalMarkerAction", (Icallback)naturalMarkerCallback);
	IupSetFunction("restartAction", (Icallback)restartCallback);
	//IupSetFunction("changeColorAction", (Icallback)changeColorCallback);
	IupSetFunction("detectionAction", (Icallback)detectionCallback);

	toolbar=IupHbox( openFile, chessboard, naturalMarker, restart, detection, IupFill(), NULL);
	//toolbar=IupHbox( openFile, chessboard, naturalMarker, restart, changeColor, detection, IupFill(), NULL);
	
	return toolbar;
}



static int azimuthSliderMove( Ihandle* ihandle )
{
	char* azimuthString = IupGetAttribute( ihandle, "VALUE" );
	float azimuthValue = (float)atof( azimuthString );
    printf( "azimuth value = %f\n", azimuthValue );
	_azimuth = azimuthValue;
	repaint_cb( canvas );
	return IUP_DEFAULT;
}



static int pitchSliderMove( Ihandle* ihandle )
{
	char* pitchString = IupGetAttribute( ihandle, "VALUE" );
	float pitchValue = (float)atof( pitchString );
	printf( "pitch value = %f\n", pitchValue );
	_pitch = pitchValue;
	repaint_cb( canvas );
	return IUP_DEFAULT;
}



static int rollSliderMove( Ihandle* ihandle )
{
	char* rollString = IupGetAttribute( ihandle, "VALUE" );
	float rollValue = (float)atof( rollString );
	printf( "roll value = %f\n", rollValue );
	_roll = rollValue;
	repaint_cb( canvas );
	return IUP_DEFAULT;
}



Ihandle* initSliders(void)
{
	Ihandle *sliderBox, *azimuth, *pitch, *roll, *azimuthLabel, * pitchLabel, *rollLabel;

	azimuth = IupVal( "HORIZONTAL" );
	pitch   = IupVal( "HORIZONTAL" );
	roll    = IupVal( "HORIZONTAL" );

	azimuthLabel = IupLabel( "Azimuth: " );
	pitchLabel   = IupLabel(   "Pitch: " );
	rollLabel    = IupLabel(    "roll: " );

	IupSetAttribute( azimuthLabel, "SIZE", "40x" );
	IupSetAttribute( pitchLabel  , "SIZE", "40x" );
	IupSetAttribute( rollLabel   , "SIZE", "40x" );

    IupSetAttribute( azimuth, "MIN", "-3.1416" );
	IupSetAttribute( pitch  , "MIN", "-1.57" );
	IupSetAttribute( roll   , "MIN", "-3.1416" );

	IupSetAttribute( azimuth, "MAX", "3.1416" );
	IupSetAttribute( pitch  , "MAX", "1.57" );
	IupSetAttribute( roll   , "MAX", "3.1416" );

	IupSetAttribute( azimuth, "SIZE", "280x15" );
	IupSetAttribute( pitch  , "SIZE", "280x15" );
	IupSetAttribute( roll   , "SIZE", "280x15" );

    IupSetAttribute( azimuth, "VALUE", "0" );
	IupSetAttribute( pitch  , "VALUE", "0" );
	IupSetAttribute( roll   , "VALUE", "0" );

	IupSetCallback( azimuth, "VALUECHANGED_CB", (Icallback)azimuthSliderMove ); 
	IupSetCallback( pitch  , "VALUECHANGED_CB", (Icallback)pitchSliderMove ); 
	IupSetCallback( roll   , "VALUECHANGED_CB", (Icallback)rollSliderMove ); 

	sliderBox = IupVbox( IupHbox(azimuthLabel, azimuth, NULL), IupHbox(pitchLabel, pitch, NULL), IupHbox(rollLabel, roll, NULL), NULL );

	return sliderBox;
}



Ihandle* InitCanvas(void)
{
	Ihandle* _canvas = IupGLCanvas("repaint_cb");        /* create _canvas and define its repaint callback */
	IupSetAttribute(_canvas,IUP_RASTERSIZE,"640x480");   /* define the size in pixels */
	IupSetAttribute(_canvas,IUP_BUFFER,IUP_DOUBLE);      /* define that this OpenGL _canvas has double buffer (front and back) */
	IupSetAttribute(_canvas, "RESIZE_CB", "resize_cb");  /* define callback action associate with the change in size of the _canvas */

	/* bind callback actions with callback functions */
	IupSetFunction("repaint_cb", (Icallback) repaint_cb);  
	IupSetFunction("resize_cb", (Icallback) resize_cb);

	return _canvas;
}



Ihandle* InitDialog(void)
{
	Ihandle* dialog;   /* dialog containing the canvas */

	Ihandle* toolbar=InitToolbar(); /* buttons tool bar */
	canvas = InitCanvas();          /* canvas to paint with OpenGL */
	Ihandle* sliders = initSliders();
	msgbar = IupLabel("A msg bar"); /* a msg bar */
	IupSetfAttribute(msgbar,"EXPAND","YES");

	/* create the dialog and set its attributes */
	dialog = IupDialog(IupVbox(/*toolbar,*/ canvas, sliders, NULL));
	IupSetAttribute(dialog, "TITLE", "IUP_OpenGL");
	IupSetAttribute(dialog, "CLOSE_CB", "exit_cb");
	IupSetFunction("exit_cb", (Icallback) exit_cb);

	IupSetFunction( "IDLE_ACTION", (Icallback)idle_cb );

	return dialog;
}



int main(int argc, char* argv[]) 
{
	Ihandle* dialog;
	IupOpen(&argc, &argv);                      /* opens the IUP lib */
	IupImageLibOpen();                          /* enable iup_image manipulation (for buttons) */
	IupGLCanvasOpen();                          /* enable the use of OpenGL to draw in canvas */

	dialog = InitDialog();                      /* local function to create a dialog with buttons and canvas */
	
	IupShowXY(dialog, IUP_CENTER, IUP_CENTER);  /* shows dialog in the center of screen */
	IupMainLoop();                              /* handle the program control to the IUP lib until a return IUP_CLOSE */
	cv::destroyAllWindows();
	IupClose();                                 /* closes the IUP lib */
	return 1;
}

